import java.util.*;
import java.util.Map.Entry;


public class AStarSearch {
	public static List<StateWithCost> solve(StateWithHeuristic start, StateWithHeuristic goal) {
		/**
		 *  Holds frontier nodes - provides access to the lowest cost node first
		 */
		PriorityQueue<PathNode> frontier = new PriorityQueue<PathNode>();

		/**
		 *  Holds a record of nodes that have already been explored.  Mapping
		 *  of <node A, node A> allows for very fast .contains() and very fast
		 *  .get() for comparison to current node and removal if necessary
		 */
		Map<PathNode,PathNode> exploredAndFrontier = new HashMap<>();
		// The total number of nodes explored.
		int nodesExplored = 0;
		// The total number of nodes that have been in the frontier.
		int nodesInFrontier = 0;
		// The total number of nodes created.
		int nodesCreated = 0;
		
		PathNode first = new PathNode(start, null, 0);		
		frontier.add(first);
		//Use the map as a quick way to see if a node exists and to acquire it
		exploredAndFrontier.put(first,first);
		nodesInFrontier++;
		
		while (!frontier.isEmpty()) {
			PathNode pathNode = frontier.remove();
			StateWithHeuristic node = pathNode.getPuzzleNode();
			nodesExplored++;
			if(node.equals(goal))
			{
				System.out.println("Nodes Explored " + nodesExplored);
				System.out.println("nodesInFrontier " + nodesInFrontier);
				System.out.println("nodesCreated " + nodesCreated);
				return getSolutionPath(pathNode); 
			}
			//get this nodes neighbors
			Map<? extends StateWithCost, Double> childMap =  node.getNeighborsWithCost();
			nodesCreated++;

			// get the list of children with costs so we can iterate over it
			Set<?> childSet = childMap.entrySet();
			Iterator<?> itr = childSet.iterator();
			while(itr.hasNext()) {
				Entry<StateWithCost, Double> next = (Entry<StateWithCost, Double>) itr.next();
				Entry<StateWithCost,Double> childEntry = next;
				StateWithHeuristic child = (StateWithHeuristic) childEntry.getKey(); // to acquire the child (neighbor) Object
				// calculate the cost (separate line for readability)
				double cost = calculateCost(pathNode, child, childMap.get(child), goal);
				PathNode newNode = new PathNode(child, pathNode,cost);
				//check to see if there's duplicate states
				if(exploredAndFrontier.containsKey(newNode)) {
					PathNode dup = exploredAndFrontier.get(newNode);
					if(newNode.getCost() < dup.getCost()) {
						// there's an existing path thats worse than this 
						// new one, so get rid of it
						exploredAndFrontier.remove(dup);
					}
					else
						// There's a duplicate but newNode has a longer path
						// don't need this child, so discard it
						newNode = null;
				}
				// if we got here and haven't discarded the child, add it 
				if(newNode != null) {
					frontier.add(newNode);
					exploredAndFrontier.put(newNode,newNode);
				} 
			} // end while(itr.hasNext())
		} // end While(!Frontier.isEmpty())
		// if we're here, we didn't find a solution
		return null;
	}// end solve() 
	/**
	 *  Determines the type of search being performed by changing the way 
	 *  the notion of cost is calculated
	 * @param currentNode Reprents the path cost so far
	 * @param child the next node in the path
	 * @param parentToChildCost cost to go from parent to child
	 * @param goal the goal state
	 * @return The cost calculated via heuristic for this search type
	 */
	private static double calculateCost(PathNode currentNode, 
			StateWithHeuristic child, 
			double parentToChildCost, StateWithHeuristic goal)	{
		return currentNode.getCost() + parentToChildCost + child.getEstimatedCostToGoal(goal);
	}
	
	/**
	 *  Reverse engineers the path using PathNode's getParent() method,
	 *  returns the path as a linked list of StateWithCost Objects
	 * @param pathNode
	 * @param parentMap
	 * @return A list of StateWithCost objects that comprise the path to the solution
	 */
	public static List<StateWithCost> getSolutionPath(PathNode pathNode) {
	LinkedList<StateWithCost> path = new LinkedList<>();
		while (pathNode != null) {
			path.addFirst(pathNode.getPuzzleNode());
			pathNode = pathNode.getParent();
		}
		return path;
	}
	
	/**
	 * Provides a wrapper for StateWithCost objects that maintains a record of
	 * the path via a link to a parent PathNode object, Maintains to total cost 
	 * of the path via a cost field, and is comparable so that it works with 
	 * the priority queue. Calculates equals and Hashcode based on the State so
	 * that behaves as desired in a hashMap for lookup based on State
	 * @author darin
	 *
	 */
	private static class PathNode implements Comparable{

		/**
		 * Hold the cost of the path so far
		 */
		double cost;
		/**
		 * Keeps a reference to the parent PathNode Object for reconstructing the path
		 */
		PathNode parent = null;
		/**
		 *  Holds the puzzle state
		 */
		StateWithHeuristic puzzleNode;
		
		public PathNode(StateWithHeuristic puzzleNode, PathNode parent, double cost) {
			this.puzzleNode = puzzleNode;
			this.parent = parent;
			this.cost = cost;
		}
		
		/**
		 * @return the cost of the path so far
		 */
		public final double getCost() {
			return cost;
		}
		
		/**
		 * @return the state
		 */
		public final StateWithHeuristic getPuzzleNode() {
			return puzzleNode;
		}
		/**
		 * 
		 * @return the previous link in the path
		 */
		public final PathNode getParent() {
			return parent;
		}
		
		/**
		 * Compares two PathNode objects based only on path cost
		 * This method is used to order objects in the priority queue
		 * @param o A PathNode object
		 * @return a negative integer if o has a longer path cost,
		 * 			a positive integer o has a shorter path cost,
		 * 			zero if path cost is equal
		 */
		@Override
		public int compareTo(Object o) {
			if(! (o instanceof PathNode))
	            throw new ClassCastException("Invalid object");
			else return (int)(getCost() - ((PathNode)o).getCost());
		}
		
		/**
		 * Compares two nodes for equality based only on state information.
		 * Operaties by calling the equals method of the StateWithHeuristic object
		 * held as puzzleNode
		 * This method is used to locate objects within a HashMap
		 * @param o
		 * @return true if both PathNodes have the same state
		 */
		public boolean equals(Object o) {
			PathNode other = (PathNode)o;
			return puzzleNode.equals(other.getPuzzleNode());	
		}
		
		/**
		 * Calculates the hashcode for this object based only on state information.
		 * Operaties by calling the hashCode() method of the StateWithHeuristic object
		 * held as puzzleNode
		 * @param o
		 * @return the hashcode of the state representation
		 */
		public int hashCode() {
			return puzzleNode.hashCode();
		}
	}
}

